/*
 * filtering_and_tracking1.h
 *
 *  Created on: Nov 24, 2022
 *      Author: xidian
 */

#ifndef FILTERING_AND_TRACKING1_H_
#define FILTERING_AND_TRACKING1_H_

#include <vector>
#include <map>
#include <queue>
#include <iostream>
#include "private_struct.h"
#include "public_struct.h"
#include "MatrixOperations.h"

class DataProcessing
{
public:
    DataProcessing();
    ~DataProcessing();

    //! processing per circle
    void processing_per_circle(std::vector<PointInfo>& input, std::vector<TrackOut>& output,int circle);

    void setFilterParams(filterChangedParams& filterChangedParams);

private:
    int init_map();
    void init_id_cache(uint32_t count)
    {
        for (uint32_t i = 1; i <= count; i++)
            m_id_cache.push(i);
    }
    void getMatrixH(float pos_x, float pos_y, float pos_z,float vel_x, float vel_y,float vel_z, float* H, int row, int column);
    void initialize();

    //! correlate_with_active_tracks()
    //! Description: correlate with active tracks
    void correlate_with_active_tracks(std::vector<PointInfo>& input,
                                      std::vector<TrackInfo>& track,
                                      std::vector<associated_struct>& output
    );

    //! correlate_with_detective_tracks()
    //! Description: correlate with detective tracks
    void correlate_with_detective_tracks(std::vector<PointInfo>& input,
                                         std::vector<TrackInfo>& track,
                                         std::vector<associated_struct>& output);

    //! correlate_with_points
    //! Description: correlate with detective points
    void correlate_with_points(std::vector<PointInfo>& input,
                               std::vector<PointTemp>& points,
                               std::vector<associated_struct>& output);

    //! update_active_track_state
    //! Description: update the states of active tracks
    void update_active_track_state(std::vector<TrackInfo>& track);

    //! update_detective_track_state
    //! Description: update the states of detective tracks
    void update_detective_track_state(std::vector<TrackInfo>& track);

    //! update_points_state()
    void update_points_state(std::vector<PointTemp>& track);

    //! delete_expired_track
    //! Description: delete expired track
    void delete_expired_detective_track(std::vector<TrackInfo>& track);

    //! delete_expired_track
    //! Description: delete expired track
    void delete_expired_active_track(std::vector<TrackInfo>& track);

    //! delete_expired_points
    //! Description: delete expired points
    void delete_expired_points(std::vector<PointTemp>& point);

    //! active_track_filtering
    //! Description: filter active tracks with points
    void active_track_filtering(std::vector<PointInfo>& input,
                                std::vector<TrackInfo>& track,
                                std::vector<associated_struct>& associated_list);

    //! unit_active_track_filtering
    //! Description: filter active track with points
    void unit_active_track_filtering(PointInfo& input, TrackInfo& track);

    //! detective_track_filtering
    //! Description: filter detective tracks with points
    void detective_track_filtering(std::vector<PointInfo>& input,
                                   std::vector<TrackInfo>& track,
                                   std::vector<associated_struct>& associated_list);

    //! unit_detective_track_filtering
    //! Description: filter detective track with points
    void unit_detective_track_filtering(PointInfo& input, TrackInfo& track);

    //! active_track_filtering_without_update
    //! Description: active track filtering without updating
    void active_track_filtering_without_update(std::vector<TrackInfo>& track);

    //! detective_track_filtering_without_update()
    //! Description: detective track filtering without updating
    void detective_track_filtering_without_update(std::vector<TrackInfo>& track);

    //! start_active_track()
    //! Description: initialize the new active track
    void start_active_track(std::vector<TrackInfo>& detective_track,
                            std::vector<associated_struct>& detective_asslist,
                            std::vector<TrackInfo>& active_track,
                            std::vector<associated_struct>& active_asslist);

    //! start_detective_track()
    //! Description: initialize the new detective track
    void start_detective_track(
            std::vector<PointInfo>& input,
            std::vector<PointTemp>& points,
            std::vector<associated_struct>& associated_list,
            std::vector<TrackInfo>& track);

    //! append_new_points()
    //! Description: append the lastest points which are not correlated with
    void append_new_points(std::vector<PointInfo>& input, std::vector<PointTemp>& output);

    //!	output_the_track()
    //! Description: output the track
    void output_the_track(std::vector<TrackInfo>& track, std::vector<TrackOut>& output);

    //! resolute_ambiguity()
    void resolute_ambiguity(std::vector<TrackInfo>& active_tarck,
                            std::vector<associated_struct>& active_asslist,
                            std::vector<TrackInfo>& detective_tarck,
                            std::vector<associated_struct>& detective_asslist);

    int f0;
    int B;
    int prt;
    int c;
    float v_mohumax_right;
    float v_mohumax_left;
    float dt;
    float m_a_inc_x;
    float m_a_inc_y;
    float m_a_inc_z;
    float m_azimuth_threshold_ref;
    float m_range_threshold_ref;
    float high_filter_error_coeff;
    int doppler_max;
    float  doppler_res;
    uint32_t m_track_id;
    uint32_t m_sector_id;
    uint32_t sector_count;
    uint32_t sector_count_front;
    uint32_t sector_count_back;
    float iir_forget_factor;
    int circle;


    const uint32_t m_max_associated_count = 20;
    std::queue<uint32_t> m_id_cache;
    /*std::queue<uint32_t> m_id_cache_temp;*/
    std::vector<TrackInfo> m_detective_track_vec;
    std::vector<TrackInfo> m_active_track_vec;
    std::vector<PointTemp> m_points_vec;
    std::vector<associated_struct> m_active_associated_list;
    std::vector<associated_struct> m_detective_associated_list;
    std::vector<associated_struct> m_points_associated_list;
    std::map<uint32_t, std::vector<TrackInfo>> m_all_active_track;
    std::map<uint32_t, std::vector<TrackInfo>> m_all_detective_track;

    MatrixOperations* m_MatrixOprt;
    KalmanFilterParameter m_KalmanFilterParam;

    int active_id;
};




#endif /* FILTERING_AND_TRACKING1_H_ */
